clear all;

COM_PORT_SERVO = 7;
COM_PORT_ARD = 6;
IR_THRESHOLD = 175;
ACCEL_DEVIATION = 15; 
X_STABLE = 79;
Y_STABLE = 97;
Z_STABLE = 155;
X_AXIS = 0;
Y_AXIS = 1;
Z_AXIS = 2;
ALPHA = 0.50;

delete(instrfind);

turtle = Franklin(COM_PORT_SERVO);
accelPort = serial(['COM', num2str(COM_PORT_ARD)]);
fopen(accelPort);
set(accelPort, 'Timeout', 1);

pause(3);
disp('Starting now');

while (true)
   fwrite(accelPort, 1); % tell the Arduino to send back accelerometer inputs
   pause(0.06);
   inByte = fread(accelPort, 3);
   %inByte
   
   irInput = turtle.getIR();
   
   if (~isempty(inByte))
       xAccel = inByte(1);
       xMoved = abs(X_STABLE - xAccel) > ACCEL_DEVIATION;
       
       yAccel = inByte(2);
       yMoved = abs(Y_STABLE - yAccel) > ACCEL_DEVIATION;
       
       zAccel = inByte(3);
       zMoved = abs(Z_STABLE - zAccel) > ACCEL_DEVIATION;
   else
       xAccel = 0;
       xMoved = 0;
       
       yAccel = 0;
       yMoved = 0;
       
       zAccel = 0;
       zMoved = 0;
   end
   
   while (irInput > IR_THRESHOLD || xMoved || yMoved || zMoved)
       turtle.retract();
       pause(3);
       
       fwrite(accelPort, 1); % tell the Arduino to send back accelerometer inputs
       pause(0.06);
       inByte = fread(accelPort, 3);
       
       if (~isempty(inByte))
            xAccel = inByte(1);
            xMoved = abs(X_STABLE - xAccel) > ACCEL_DEVIATION;
       
            yAccel = inByte(2);
            yMoved = abs(Y_STABLE - yAccel) > ACCEL_DEVIATION;
       
            zAccel = inByte(3);
            zMoved = abs(Z_STABLE - zAccel) > ACCEL_DEVIATION;
       else
           xAccel = 0;
           xMoved = 0;
       
           yAccel = 0;
           yMoved = 0;
       
           zAccel = 0;
           zMoved = 0;
       end
       
       irInput = turtle.getIR();
   end
   
   turtle.expand();
end

% TODO use slope as threshold instead